# @FileName  : scara.py
# @Time      : 2025/4/23 10:20
# @Author    : LuZhaoHui
# @Software  : PyCharm

'''
    运动：
        指令： G0 快速移动（无插补）
        参数： XYZIF
        格式： G0 X100 Y0 Z0 I0 F8000
        说明：快速移动到坐标100,0,0，I关节位置0,0,0，速度8000mm/分钟

        指令： G28 回原点
        参数： XYZI
        格式： G28 X0 Y0 Z0 I0
        说明： 4个轴依次开始旋转搜寻原点开关直到触发开关停止，并设置参数数据为原点位置
        注意使用这个指令前要确保原点开关安装好能正常触发，如果任意

        指令： G92 设置电机位置
        参数： XYZI
        格式： G92 X0 Y0 Z0 I0
        说明： 4个轴设置位置值

    控制：
        指令： M19 电机急停
        参数： 无
        格式： M19
        说明： 执行后，断开连接

    辅助：
        指令： M114 返回当前位置
        参数： 无
        格式： M114
        说明： 返回当前设备的位置信息

        指令： M143 返回当前位置
        参数： SVP
        格式： M143
        说明： 判断移动是否完成，完成时串口返回字符串"Arrival position"

    输出：
        指令： M130-139 输出信号1-10
        参数： V
        格式： M130 V
        说明： V0 关，V1 开


    连接：
        M371
        M358 V1
        M361

注意：
    移动臂：X,Y,Z 3个电机
    旋转：I 电机
    不能同时工作，只能合成一条指令，可速度不可以

    通电后：连接后注意当前位置，
    连接后：第一条指令只能是原点=移动至原点+设置原点为初始位置

原点  =   X92.986 Y73.468,190.000
位置1 =   X-101.710 Y249.710 Z190.000
         X-101.710 Y249.710 Z150.000
位置2 =   X63.570 Y247.430 Z190.000
         X63.570 Y247.430 Z150.000
位置3 =   X213.290 Y164.860 Z190.000
         X213.290 Y164.860 Z150.000
位置4 =   X269.14 Y4.14 Z190.0 Z150.0
         X269.14 Y4.14 Z150.0
位置5 =   X210.86 Y-163.43 Z190.0
         X210.86 Y-163.43 Z150.0
位置6 =   X65.57 Y-254.14 Z190.0
         X65.57 Y-254.14 Z150.0
位置7 =   X-101.430 Y-240.000 Z190.000
         X-101.430 Y-240.000 Z150.000

旋转  =   I1000







'''

# 指令格式
CODEDICT = {
    'code': '',
}

MOVESTR = '移动'
QUAKESTR = '震动'
J4STR = 'J4'
OUTSTR = '输出'
DELAYSTR = '计时'
STOPSTR = '暂停'
ARMSTR = '换臂'
WAITSTR = '等待'

def send_link():
    # 连接指令
    return ['M371', 'M358 V1', 'M361']


def send_stop():
    return 'M19'


def send_arm(mode):
    return ['M358 V%d' % mode]


def send_out(out, sw):
    return ['M13%d V%d' % (out, sw)]


def send_move0(point, rate=3000):
    g28 = 'G28 X%.3f Y%.3f Z%.3f F%d' % (point[0], point[1], point[2], rate)
    g92 = 'G92 X%.3f Y%.3f Z%.3f' % (point[0], point[1], point[2])
    return [g28, g92, 'M121']


def send_move(point, rate=None, m143=False):
    g0 = 'G0 X%.3f Y%.3f Z%.3f' % (point[0], point[1], point[2])
    if rate is not None:
        g0 += ' F%d' % rate
    if m143:
        return [g0, 'M143']
    else:
        return [g0]


def send_move2(point, step, rate, g92=False, m143=False):
    g0 = 'G0'
    if len(point) >=3:
        g0 += ' X%.3f Y%.3f Z%.3f' % (point[0], point[1], point[2])
    if step > 0:
        g0 += ' I%d' % step
    g0 += ' F%d' % rate
    if m143:
        if g92:
            return ['G92 I0', g0, 'M143']
        else:
            return [g0, 'M143']
    else:
        if g92:
            return ['G92 I0', g0]
        else:
            return [g0]


def send_locate():
    return ['M114']


def send_rotate_stop():
    return ['G92 I0']


def getPoint1(point):
    return point[:3]


def getPoint2(point):
    return point[:2] + point[3:]


def isActType(act):
    s = act
    if s[:len(MOVESTR)] == MOVESTR:
        return 0
    elif s[:len(QUAKESTR)] == QUAKESTR:
        return 1
    elif s[:len(J4STR)] == J4STR:
        return 2
    elif s[:len(OUTSTR)] == OUTSTR:
        return 3
    elif s[:len(DELAYSTR)] == DELAYSTR:
        return 4
    elif s[:len(STOPSTR)] == STOPSTR:
        return 5
    elif s[:len(ARMSTR)] == ARMSTR:
        return 6
    elif s[:len(WAITSTR)] == WAITSTR:
        return 7
    return -1


def isCodeType(act):
    act_s = act.strip()
    if len(act_s) <= 0 or act_s[0] == ';':
        return -1, None
    a = eval(act_s)
    while type(a) == dict:
        if 'code' not in a.keys():
            break
        s = a['code']
        if s[:len(MOVESTR)] == MOVESTR:
            return 0, a
        elif s[:len(QUAKESTR)] == QUAKESTR:
            return 1, a
        elif s[:len(J4STR)] == J4STR:
            return 2, a
        elif s[:len(OUTSTR)] == OUTSTR:
            return 3, a
        elif s[:len(DELAYSTR)] == DELAYSTR:
            return 4, a
        elif s[:len(STOPSTR)] == STOPSTR:
            return 5, a
        elif s[:len(ARMSTR)] == ARMSTR:
            return 6, a
        elif s[:len(WAITSTR)] == WAITSTR:
            return 7, a
    return -1, None
